#include "RenderUtils.h"

int delta = 0;
void *font = GLUT_BITMAP_TIMES_ROMAN_24;
char s[1000];

char buff1[8];
char buff2[8];
void SaveAllBuffers(int w, int h, int numCam, int numFrame) {

	int thesize = sizeof(char) * w * h * 3;
	char* pixels = (char*)malloc( thesize );
	CvMat* img = cvCreateMat(h,w,CV_8UC3);
	_itoa_s(numFrame, buff2, 8,10); // save number of frame
	string frameend(buff2);
	string frame;
	if( numFrame < 10 )
		frame.append("0");
	if( numFrame < 100 )
		frame.append("0");
	frame.append(frameend);
	int initWindowID = glutGetWindow(); 
	for( int idx = 0; idx < numCam; idx++ ) {
		_itoa_s(idx, buff1,8, 10 ); // save number of camera
		string end(buff1);
		glutSetWindow(idx+1);
		glReadPixels( 0, 0, w, h, GL_RGB, GL_UNSIGNED_BYTE, pixels );
		int k = 0;
		for( int i = h-1; i >= 0; i-- ) {
			for( int j = 0; j < w; j++ ) {
				uchar r = (uchar) pixels[k];
				uchar g = (uchar) pixels[k+1];
				uchar b = (uchar) pixels[k+2];
				CvScalar scalar = cvScalar(b,g,r);
				cvSet2D(img,i,j,scalar);
				k+=3;
			}
		}
		string name = "ReadBufferWindow";
		name.append(end);
		name.append(string("_frame"));
		name.append(frame);
		string fname = ".png";
		name.append(fname);
		cvSaveImage( name.c_str(), img );
		glFinish();
		int breakhere = 1;

	}
	cvReleaseMat( &img );
	free( pixels );
	glutSetWindow(initWindowID);

}

CvMat* GetBuffer(int w, int h, int numCam ) {


	int thesize = sizeof(char) * w * h * 3;
	char* pixels = (char*)malloc( thesize );
	CvMat* img = cvCreateMat(h,w,CV_8UC3);
	
	int initWindowID = glutGetWindow(); 
	int idx = numCam;
	
	glutSetWindow(idx);
	glReadPixels( 0, 0, w, h, GL_RGB, GL_UNSIGNED_BYTE, pixels );
	int k = 0;
	for( int i = h-1; i >= 0; i-- ) {
		for( int j = 0; j < w; j++ ) {
			uchar r = (uchar) pixels[k];
			uchar g = (uchar) pixels[k+1];
			uchar b = (uchar) pixels[k+2];
			CvScalar scalar = cvScalar(b,g,r);
			cvSet2D(img,i,j,scalar);
			k+=3;
		}
	}
	
	glFinish();
	free( pixels );
	glutSetWindow(initWindowID);
	return img;
}

void TestReadBuffer(int h, int w)
{
	int thesize = sizeof(char) * w * h * 3;
	char* pixels = (char*)malloc( thesize );
	glReadPixels( 0, 0, w, h, GL_RGB, GL_UNSIGNED_BYTE, pixels );
	IplImage* img = cvCreateImage( cvSize(w,h), IPL_DEPTH_8U, 3 );
	cvZero(img);
	int step       = img->widthStep/sizeof(uchar);
	int channels   = img->nChannels;
	uchar* data    = (uchar *)img->imageData;
	
	for( int k = 0; k < thesize; k+=3 ) {
		int i = h - k / 3 / h - 1;
		int j = (k / 3) % h;
		int r = (int) pixels[k];
		int g = (int) pixels[k+1];
		int b = (int) pixels[k+2];
		data[i*step+j*channels+0]=b; // B
		data[i*step+j*channels+1]=g; // G
		data[i*step+j*channels+2]=r; // R
		int breakehre = 1;
	}
	cvSaveImage( "TestReadBuffer.png", img );
	cvReleaseImage( &img );
	free( pixels );
	int breakhere = 1;
}

void DrawAxes()
{
	// Draw and Label Axes
    glColor3f (0.0, 1.0, 1.0);
	glBegin(GL_LINES);
                glVertex3f(-1.8f, 0.0f,0.0f);
                glVertex3f(1.8f, 0.0f,0.0f);
    glEnd();
    glBegin(GL_LINES);
                glVertex3f(0.0f, -1.8f,0.0f);
                glVertex3f(0.0f, 1.8f,0.0f);
    glEnd();
    glBegin(GL_LINES);
                glVertex3f(0.0f, 0.0f, -1.8f);
                glVertex3f(0.0f, 0.0f, 1.8f);
    glEnd();

    sprintf(s,"+z");
    outputCharacter(0.0f, 0.0f, 1.98f, s);
    sprintf(s,"-z");
    outputCharacter(0.0f, 0.0f, -1.98f, s);
    sprintf(s,"+x");
    outputCharacter(1.9f, 0.0f, 0.0f, s);
    sprintf(s,"-x");
    outputCharacter(-1.9f, 0.0f, 0.0f, s);
    sprintf(s,"+y");
    outputCharacter(0.0f, 1.9f, 0.0f, s);
    sprintf(s,"-y");
    outputCharacter(0.0f, -1.99f, 0.0f, s);
}

void TestGLRenderNoMV()
{
	delta = delta % 360 + 1;
	
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	// Don't reset model view
	// The Camera(S) will set the view with gluLookAt()
	// Just draw the geometry
	
	/*glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();*/

	glPushMatrix();
	glRotatef(45.0,0.0,1.0,0.0);
	glScalef(2.0,2.0,2.0);
	glTranslatef(-0.5,-0.5,0.0);
	glColor3f(1.0,1.0,1.0);
	glBegin(GL_POLYGON);
		glVertex3f(0.25, 0.25, 0.0);
		glVertex3f(0.75, 0.25, 0.0);
		glVertex3f(0.75, 0.75, 0.0);
		glVertex3f(0.25, 0.75, 0.0);
	glEnd();
	glPopMatrix();
	

	DrawAxes();

}

void TestGLRender()
{
	delta = delta % 360 + 1;
	
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	glPushMatrix();
	glRotatef(0,1.0,0.0,0.0);
	glColor3f(1.0,1.0,1.0);
	glBegin(GL_POLYGON);
		glVertex3f(0.25, 0.25, 0.0);
		glVertex3f(0.75, 0.25, 0.0);
		glVertex3f(0.75, 0.75, 0.0);
		glVertex3f(0.25, 0.75, 0.0);
	glEnd();
	glPopMatrix();

	// label axes
    glColor3f (0.0, 1.0, 1.0);
    sprintf(s,"+z");
    outputCharacter(0.0f, 0.0f, 1.98f, s);
    sprintf(s,"-z");
    outputCharacter(0.0f, 0.0f, -1.98f, s);
    sprintf(s,"+x");
    outputCharacter(1.9f, 0.0f, 0.0f, s);
    sprintf(s,"-x");
    outputCharacter(-1.9f, 0.0f, 0.0f, s);
    sprintf(s,"+y");
    outputCharacter(0.0f, 1.9f, 0.0f, s);
    sprintf(s,"-y");
    outputCharacter(0.0f, -1.99f, 0.0f, s);

	
}

void outputCharacter(float x, float y, float z, char *string) {
  int len, i;
  glRasterPos3f(x, y, z);
  len = (int) strlen(string);
  for (i = 0; i < len; i++) {
    glutBitmapCharacter(font, string[i]);
  }
}

